import json
import rclpy
from rclpy.node import Node
from std_msgs.msg import String, Char

# local
from AGV_F50A_protocol import *

# Sim_Cmd_Control = False
Sim_Cmd_Control = True


class Agv_Ros(Node):
    def __init__(self, AgvServer):
        super().__init__("agv_server")
        self.AgvServer = AgvServer
        self.agv_impl = AgvServer.agv_impl
        self.sub_tcp = self.create_subscription(
            String, "/tcp_server/cmd", self.listener_callback, 10
        )
        # Test:命令控制
        if Sim_Cmd_Control:
            self.control_count = 0
            self.control_client_id = 0
            self.control_mode = 0
            self.sub_cmd = self.create_subscription(String, "/cmd", self.control_cmd, 2)
            self.pub_charging_cmd = self.create_publisher(
                String, "/charger_server/cmd", 2
            )

    def listener_callback(self, msg):
        self.get_logger().info('I heard: "%s"' % msg.data)

    def choose_client_id(self, AGV_impl):
        clients = []
        idx = 0
        for key in AGV_impl.Client_Set:
            print(f"idx:{idx}, client_id: 0x{key:02X}")
            idx += 1
            clients.append(key)

        clnt_num = len(clients)
        if clnt_num == 0:
            print("没有连接的客户端")
            return 0
        elif clnt_num == 1:
            idx = 0
        else:
            print("请输入 idx")
            input_str = input().strip()
            idx = int(input_str)

        self.control_client_id = clients[idx]
        print(f"设置为client_id: {self.control_client_id:02X}")
        return self.control_client_id

    def signal_cmd_mode(self, key, agv_impl):
        help_str = """
1 二维码地图模式, 2 signal_cmd单命令模式
h 查看帮助 p 选择client
移动
    w 前行
a 左    d 右

r 进入休眠, f 退出休眠
t 原地校准, g 旋转标定
空格 暂停, / 继续, ,终止动作
"""
        if self.control_count % 10 == 0:
            print(help_str)
        self.control_count += 1
        client_id = self.control_client_id

        if key in ("0", "1", "2"):
            self.control_mode = int(key)
            self.control_count = 0
        elif key == "h":
            print(help_str)
        elif key == "p":
            self.choose_client_id(agv_impl)
        elif key == "w":
            agv_impl.hal_send_SignalCmd(client_id, E_SignalCmd.Move_Line, 300)
        elif key == "a":
            agv_impl.hal_send_SignalCmd(client_id, E_SignalCmd.WheelSteering, 345)
        elif key == "d":
            agv_impl.hal_send_SignalCmd(client_id, E_SignalCmd.WheelSteering, 15)
        elif key == "y":
            agv_impl.hal_send_SignalCmd(client_id, E_SignalCmd.Lift_Up, 200)
        elif key == "h":
            agv_impl.hal_send_SignalCmd(client_id, E_SignalCmd.Lift_Up, 0)
        elif key == "r":
            agv_impl.hal_send_SignalCmd(client_id, E_SignalCmd.EntrySleep, 0)
        elif key == "f":
            agv_impl.hal_send_SignalCmd(client_id, E_SignalCmd.ExitSleep, 0)
        elif key == "t":
            agv_impl.hal_send_SignalCmd(client_id, E_SignalCmd.InSitu_calibration, 20)
        elif key == "g":
            agv_impl.hal_send_SignalCmd(client_id, E_SignalCmd.Rotation_calibration, 0)
        elif key == " ":
            agv_impl.hal_send_SignalCmd(client_id, E_SignalCmd.Pause, 0)
        elif key == "/":
            agv_impl.hal_send_SignalCmd(client_id, E_SignalCmd.Resume, 0)
        elif key == ",":
            agv_impl.hal_send_SignalCmd(client_id, E_SignalCmd.Cancel, 0)
        else:
            print(f"未知key: {key}")

    def path_cmd_mode(self, key, agv_impl):
        help_str = """
1 二维码地图模式, 2 signal_cmd单命令模式
h 查看帮助 p 选择client
转向
   i 0度
 j 270度    l 90度
   k 180度

[ 抬升, ] 下降

移动
y 2-3, u 3-2
r 路径移动: 2 -> 3 -> 4
f 路径回退: 4 -> 3 -> 2
t 路径移动: 2 -> 3 -> 6 -> 7
g 路径回退: 7 -> 6 -> 3 -> 2
v 简短指令: 2,3,4,5,6,7,2
o 读取path.txt中的路径并执行
空格 暂停, / 继续, ,终止动作

充电
7 对接,  9 充电, 6 结束
"""
        if self.control_count % 10 == 0:
            print(help_str)
        self.control_count += 1
        client_id = self.control_client_id

        if key in ("0", "1", "2"):
            self.control_mode = int(key)
            self.control_count = 0
        elif key == "h":
            print(help_str)
        elif key == "p":
            self.choose_client_id(agv_impl)
        elif key in ("i", "k", "j", "l"):
            client = agv_impl.Client_Set[client_id]
            cur_address_id = client.realtime_state.agv_address.id
            path_action = agv_impl.gen_QRCodePath_by_shortCmd(
                "{}{}{}#".format(cur_address_id, key, cur_address_id)
            )
            agv_impl.hal_update_path(client_id, path_action)
        elif key in "[":
            client = agv_impl.Client_Set[client_id]
            cur_address_id = client.realtime_state.agv_address.id
            path_action = agv_impl.gen_QRCodePath_by_shortCmd(
                "{}^{}#".format(cur_address_id, cur_address_id)
            )
            agv_impl.hal_update_path(client_id, path_action)
        elif key in "]":
            client = agv_impl.Client_Set[client_id]
            cur_address_id = client.realtime_state.agv_address.id
            path_action = agv_impl.gen_QRCodePath_by_shortCmd(
                "{}v{}#".format(cur_address_id, cur_address_id)
            )
            agv_impl.hal_update_path(client_id, path_action)
        elif key in "y":
            path_action = agv_impl.gen_QRCodePath_by_shortCmd("2i2-3#")
            agv_impl.hal_update_path(client_id, path_action)
        elif key in "u":
            path_action = agv_impl.gen_QRCodePath_by_shortCmd("3-2#")
            agv_impl.hal_update_path(client_id, path_action)
        elif key in "r":
            path_action = agv_impl.gen_QRCodePath_by_shortCmd("2k2i2-3-4#")
            agv_impl.hal_update_path(client_id, path_action)
        elif key in "f":
            path_action = agv_impl.gen_QRCodePath_by_shortCmd("4k4-3-2i2#")
            agv_impl.hal_update_path(client_id, path_action)
        elif key in "t":
            path_action = agv_impl.gen_QRCodePath_by_shortCmd("2i2-3^3j3=6k6-7#")
            agv_impl.hal_update_path(client_id, path_action)
        elif key in "g":
            path_action = agv_impl.gen_QRCodePath_by_shortCmd("7v7i7-6l6=3k3-2i2#")
            agv_impl.hal_update_path(client_id, path_action)
        elif key in "v":
            path_action = agv_impl.gen_QRCodePath_by_shortCmd(
                "2i2-3^3-4j4=5k5-6-7v7l7=2i2#"
            )
            agv_impl.hal_update_path(client_id, path_action)
        elif key == " ":
            agv_impl.hal_send_SignalCmd(client_id, E_SignalCmd.Pause, 0)
        elif key == "/":
            agv_impl.hal_send_SignalCmd(client_id, E_SignalCmd.Resume, 0)
        elif key == ",":
            agv_impl.hal_send_SignalCmd(client_id, E_SignalCmd.Cancel, 0)
        elif key in "7":
            batter_info = self.agv_impl.hal_get_agv_batteryInfo(client_id)
            batter_info["cmd"] = "connect"
            batter_info["station_id"] = 754
            msg = String()
            msg.data = json.dumps(batter_info)
            self.pub_charging_cmd.publish(msg)
        elif key in "8":
            self.agv_impl.hal_charge_cmd(client_id, True)
        elif key in "9":
            self.agv_impl.hal_charge_cmd(client_id, False)
        else:
            print(f"未知key: {key}")

    def control_cmd(self, msg):
        help_str = """
1 二维码地图模式, 2 signal_cmd单命令模式
h 查看帮助 p 选择client
"""
        agv_impl = self.AgvServer.agv_impl
        client_id = self.control_client_id
        key = msg.data[0]
        if self.control_mode == 1:
            self.path_cmd_mode(key, agv_impl)
            return
        elif self.control_mode == 2:
            self.signal_cmd_mode(key, agv_impl)
            return

        if self.control_count % 10 == 0:
            print(help_str)
        self.control_count += 1

        if key in ("0", "1", "2"):
            self.control_mode = int(key)
        elif key == "h":
            print(help_str)
        elif key == "p":
            self.choose_client_id(agv_impl)
        else:
            print(f"未知key: {key}")
